/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package heuristics;

import algos.Geometry;
import java.awt.*;
import java.awt.geom.*;
import java.io.File;
import java.io.FileWriter;
import java.util.ArrayList;
import java.util.Arrays;

import misc.FOV;

/**
 * 
 * @author Moamen Mokhtar
 */
public class HeuristicSimulation extends CoverageHeuristic {

    final int ITERATIONS = 1;
    final int REFRESH_RATE = 10;
    int numPersons, minDistance;
    CoverageHeuristic coverageAlgo;
    ArrayList<CoverageHeuristic> coverageAlgos = new ArrayList<CoverageHeuristic>();
    Point2D.Double[] anchors;
    int[] personCoverage;
    int[] frameCoverage;
    int[] steps;
    float speed = 10;
    double[][] prev_FOV_edge;
    ArrayList<Double> deltaThetaArray;
    int numOrientations = 8;
    int num_itr_stab = 0;

    public HeuristicSimulation(String filename) {
        setup(filename);
        // add algorithms
//        coverageAlgos.add(new CentralizedForceCoverage(obstacles, cameras, people, numOrientations));
        coverageAlgos.add(new AngularSweep(obstacles, cameras, people, number_of_men));
    }

    public double getDeltaAngle() {
        return 2 * Math.PI / numOrientations;
    }

    public ArrayList<Double> getThetaArray() {
        return deltaThetaArray;
    }

    public double computeAvgStability() {
        double avgStab = 0.0;
        for (int i = 0; i < deltaThetaArray.size(); i++) {
            avgStab += deltaThetaArray.get(i);
        }
        avgStab /= (deltaThetaArray.size() * 1.0);
        return avgStab;
    }

    public double[][] simulate(int mapNum) {
        double[][] cvg = new double[coverageAlgos.size()][5];
        for (int algoId = 0; algoId < coverageAlgos.size(); algoId++) {
            continuous = coverageAlgos.get(algoId).continuous;
            prev_FOV_edge = new double[number_of_cameras][1];
            deltaThetaArray = new ArrayList<Double>();
            anchors = new Point2D.Double[number_of_men];
            steps = new int[number_of_men];
            personCoverage = new int[number_of_men];
            frameCoverage = new int[ITERATIONS];
            coverageAlgo = coverageAlgos.get(algoId);
            double avg = 0;
            num_itr_stab = 0;
            for (int itr = 0; itr < ITERATIONS; itr++) {
                if (itr % REFRESH_RATE == 0) {
                    reassign();
                    computedeltaThetaArray(itr / REFRESH_RATE);
                    num_itr_stab++;
//                    coverageAlgo.saveImage(mapNum, assignment, itr,0,0);
                }
                int c = coverageAlgo.computeCoverage();
                frameCoverage[itr] = c;
                avg += c;
                move();
            }
            int minCov = Integer.MAX_VALUE;
            int maxCov = 0;
            for (int i = 0; i < personCoverage.length; i++) {
                if (personCoverage[i] >= maxCov) {
                    maxCov = personCoverage[i];
                }
                if (personCoverage[i] <= minCov) {
                    minCov = personCoverage[i];
                }
            }
            double stdDev = 0;
            avg /= ITERATIONS;
            double s1 = 0;
            double s2 = 0;

            for (int i = 0; i < ITERATIONS; i++) {
                s1 += Math.pow(frameCoverage[i], 2);
                s2 += frameCoverage[i];
            }
            s1 /= ITERATIONS;
            s2 /= ITERATIONS;
            s2 = Math.pow(s2, 2);
            stdDev = Math.sqrt(s1 - s2);

            cvg[algoId] = new double[]{avg, stdDev, minCov, maxCov, computeAvgStability()};

        }
        return cvg;
    }

    private void move() {
        ML:
        for (int i = 0; i < number_of_men; i++) {
            Point2D.Double pos = people.get(i);
            if (steps[i] == 0) {
                Point2D.Double pos2 = new Point2D.Double(pos.x, pos.y);
                float distance = 0;
                int ccc = 0;
                WL:
                while (true) {
                    pos2.x = rand.nextInt(TestCoverage.width);
                    pos2.y = rand.nextInt(TestCoverage.height);
                    if (ccc++ > 20) {
                        continue ML;
                    }
                    distance = (float) pos.distance(pos2);
                    if (distance < 10) {
                        continue;
                    }
                    Line2D.Float r = new Line2D.Float(pos, pos2);
                    for (Rectangle rect : obstacles) {
                        if (rect.contains(pos2) || r.intersects(rect)) {
                            continue WL;
                        }
                    }
                    break;
                }
                pos2.x = (pos2.x - pos.x) / distance * speed;
                pos2.y = (pos2.y - pos.y) / distance * speed;
                anchors[i] = pos2;
                steps[i] = (int) Math.floor(distance / speed);
            }
            pos.x += anchors[i].x;
            pos.y += anchors[i].y;
            steps[i]--;
        }
    }

    public void computedeltaThetaArray(int itr) {
        for (int camIndex = 0; camIndex < number_of_cameras; camIndex++) {
            double angle;
            if (continuous) {
                if (itr > 0) {
                    angle = Geometry.computeAngle(
                            new double[]{cameras.get(camIndex).x, cameras.get(camIndex).y, 0},
                            new double[]{people.get(assignment[camIndex]).x,
                                people.get(assignment[camIndex]).y, 0}, prev_FOV_edge[camIndex]);
                    angle = Math.toDegrees(angle);
                    deltaThetaArray.add(angle);
                }
                prev_FOV_edge[camIndex] = new double[]{people.get(assignment[camIndex]).x,
                            people.get(assignment[camIndex]).y, 0};
            } else {

                angle = assignment[camIndex] * getDeltaAngle();
                Point2D.Double[] pArr = {new Point2D.Double(cameras.get(camIndex).x,
                    cameras.get(camIndex).y), new Point2D.Double(cameras.get(camIndex).x + 710,
                    cameras.get(camIndex).y), new Point2D.Double(cameras.get(camIndex).x + 505,
                    cameras.get(camIndex).y + 505)
                };
                AffineTransform t = new AffineTransform();
                t.rotate(angle, cameras.get(camIndex).x, cameras.get(camIndex).y);
                t.transform(pArr, 0, pArr, 0, 3); // applying transformation to   the FOV triangle
                if (itr > 0) {
                    angle = Geometry.computeAngle(new double[]{cameras.get(camIndex).x,
                                cameras.get(camIndex).y, 0},
                            new double[]{pArr[1].x, pArr[1].y, 0}, prev_FOV_edge[camIndex]);
                    angle = Math.toDegrees(angle);
                    deltaThetaArray.add(angle);
                }
                prev_FOV_edge[camIndex] = new double[]{pArr[1].x, pArr[1].y, 0};
            }
        }
    }

    public double computeRelaxedCoverage() {
        double ret = 0;
        for (int p = 0; p < number_of_men; p++) {
            if (personCamera[p] == -1) {
                continue;
            }
            int camInd = personCamera[p];
            FOV isovist = cameraIsoVist[camInd];
            Point2D.Double dir = new Point2D.Double();
            dir.x = isovist.xpoints[1] + isovist.xpoints[2]
                    - isovist.xpoints[0];
            dir.y = isovist.ypoints[1] + isovist.ypoints[2]
                    - isovist.ypoints[0];
            Point2D.Double person = new Point2D.Double(people.get(p).x
                    - cameras.get(camInd).x, people.get(p).y
                    - cameras.get(camInd).y);
            double dot = dir.x * person.x + dir.y * person.y;
            dot /= dir.distance(0, 0);
            dot /= person.distance(0, 0);
            double angle = Math.acos(dot);
            ret += Math.exp(2 * (Math.cos(angle) - 1));
        }
        return ret;
    }

    public int[] reassign() {
        assignment = coverageAlgo.reassign();
        Arrays.fill(personCamera, -1);
        Arrays.fill(camDistance, Double.POSITIVE_INFINITY);
        for (int i = 0; i < number_of_cameras; i++) {
            if (continuous) {
                double angle = Geometry.computeAngle(
                        new double[]{cameras.get(i).x, cameras.get(i).y, 0},
                        new double[]{people.get(assignment[i]).x, people.get(assignment[i]).y, 0},
                        new double[]{640, cameras.get(i).y, 0});

                if (cameras.get(i).y > people.get(assignment[i]).y) {
                    angle = 2 * Math.PI - angle;
                }
                cameraIsoVist[i] = coverageAlgo.getAngleIsovist(i, angle,0,0);
                personCamera[assignment[i]] = i;
            } else {
                cameraIsoVist[i] = coverageAlgo.getOrientationIsovist(i,
                        assignment[i]);
            }
            for (int j = 0; j < number_of_men; j++) {
                if (!cameraIsoVist[i].contains(people.get(j))) {
                    continue;
                }
                // TODO: minimize angular offset for relaxation method, not
                // distance.
                // Decreasing the offset will give a higher value of relaxed
                // coverage.
                double distance = people.get(j).distance(cameras.get(i));

                if (picks(i, j) && Double.compare(distance, camDistance[j]) < 0) {
                    camDistance[j] = distance;
                    personCamera[j] = i;
                }
            }
        }
        return assignment;
    }

    int getNumAlgos() {
        return coverageAlgos.size();
    }

    @Override
    public String toString() {
        return coverageAlgo.toString();
    }


}
